#include "../ComputeEdgeGradient.h"

#include "EstimateFunction.h"


#include <stdio.h>
#include <float.h>
#include <memory.h>
#include <math.h>


float ComputeGradient(float *function, CVector3F &gradient)
{
	float dX[3];
	float dY[3];
	float dZ[3];

	dX[0] = function[0];
	dX[1] = function[3];
	dX[2] = function[6];
	dY[0] = function[1];
	dY[1] = function[4];
	dY[2] = function[7];
	dZ[0] = function[2];
	dZ[1] = function[5];
	dZ[2] = function[8];

	gradient.m_x = dX[0] * dX[0] + dX[1] * dX[1] + dX[2] * dX[2];
	gradient.m_y = dY[0] * dY[0] + dY[1] * dY[1] + dY[2] * dY[2];
	gradient.m_z = dZ[0] * dZ[0] + dZ[1] * dZ[1] + dZ[2] * dZ[2];

	float strength = sqrt(gradient.m_x + gradient.m_z + gradient.m_y);
	gradient.m_x = sqrt(gradient.m_x);
	gradient.m_y = sqrt(gradient.m_y);
	gradient.m_z = sqrt(gradient.m_z);

	return strength;
}



CVector3F EdgeGradientMethodFTLE(CFiber **fibers, const int pos, const float delta)
{
	CPoint3F forwardPos[26];
	CPoint3F backwardPos[26];
	CPoint3F x[26];

	CVector3F dir = fibers[0]->m_seed.dir;

	for (int i = 0; i < 26; ++i) {
		x[i] = fibers[i+1]->m_seed.pos;
		CVector3F temp_dir = fibers[i+1]->m_seed.dir;
		if (InnerProduct(dir, temp_dir) > 0.0f ) {
			int index = (fibers[i+1]->m_fCount-1) > pos ? pos : (fibers[i+1]->m_fCount-1);
			forwardPos[i] = fibers[i+1]->m_pF[index];

			index = (fibers[i+1]->m_bCount-1) > pos ? pos : (fibers[i+1]->m_bCount-1);
			backwardPos[i] = fibers[i+1]->m_pB[index];
		} else {
			int index = (fibers[i+1]->m_fCount-1) > pos ? pos : (fibers[i+1]->m_fCount-1);
			backwardPos[i] = fibers[i+1]->m_pF[index];

			index = (fibers[i+1]->m_bCount-1) > pos ? pos : (fibers[i+1]->m_bCount-1);
			forwardPos[i] = fibers[i+1]->m_pB[index];
		}

	}

	float *forwardFunction = new float[3*3];
	memset(forwardFunction, 0, sizeof(float)*3*3);
	EstimateFunction(forwardPos, x, 26, 1, forwardFunction);

	float *backwardFunction = new float[3*3];
	memset(backwardFunction, 0, sizeof(float)*3*3);
	EstimateFunction(backwardPos, x, 26, 1, backwardFunction);

	// compute gradient
	CVector3F forwardGradient, backwardGradient;
	float forwardStrength = ComputeGradient(forwardFunction, forwardGradient);
	float backwardStrength = ComputeGradient(backwardFunction, backwardGradient);


	delete[] forwardFunction;
	delete[] backwardFunction;

	if (forwardStrength > backwardStrength)
		return forwardGradient;
	else 
		return backwardGradient;
}